#!/usr/bin/env python
PACKAGE = "tsdf_localization"

from dynamic_reconfigure.parameter_generator_catkin import *

gen = ParameterGenerator()

gen.add("number_of_particles", int_t, 0, "Defines the number of particles created per step", 100000, 0, 1000000)

gen.add("init_sigma_x", double_t, 0, "The sigma for the normal distribution used for initializing the x coordinate of the particles", 0.5, 0, 10)
gen.add("init_sigma_y", double_t, 0, "The sigma for the normal distribution used for initializing the y coordinate of the particles", 0.5, 0, 10)
gen.add("init_sigma_z", double_t, 0, "The sigma for the normal distribution used for initializing the z coordinate of the particles", 0.5, 0, 10)

gen.add("init_sigma_roll", double_t, 0, "The sigma for the normal distribution used for initializing the roll of the particles (in degrees)", 0, 0, 360)
gen.add("init_sigma_pitch", double_t, 0, "The sigma for the normal distribution used for initializing the pitch of the particles (in degrees)", 0, 0, 360)
gen.add("init_sigma_yaw", double_t, 0, "The sigma for the normal distribution used for initializing the yaw of the particles (in degrees)", 30, 0, 360)

gen.add("delta_update_dist", double_t, 0, "Moved distance at which a resampling step should executed", 0.03, 0, 10)
gen.add("delta_update_angle", double_t, 0, "rotated angle at which a resampling step should executed (in degrees)", 5, 0, 360)


gen.add("use_cuda", bool_t, 0, "Using cuda for the sensor-update", True)

gen.add("a_1", double_t, 0, "Parameter for the motion model", 1.0, 0.0, 30.0)
gen.add("a_2", double_t, 0, "Parameter for the motion model", 0.0, 0.0, 30.0)
gen.add("a_3", double_t, 0, "Parameter for the motion model", 1.0, 0.0, 30.0)
gen.add("a_4", double_t, 0, "Parameter for the motion model", 0.0, 0.0, 30.0)
gen.add("a_5", double_t, 0, "Parameter for the motion model", 1.0, 0.0, 30.0)
gen.add("a_6", double_t, 0, "Parameter for the motion model", 0.0, 0.0, 30.0)
gen.add("a_7", double_t, 0, "Parameter for the motion model", 0.0, 0.0, 30.0)
gen.add("a_8", double_t, 0, "Parameter for the motion model", 1.0, 0.0, 30.0)
gen.add("a_9", double_t, 0, "Parameter for the motion model", 0.0, 0.0, 30.0)
gen.add("a_10", double_t, 0, "Parameter for the motion model", 1.0, 0.0, 30.0)
gen.add("a_11", double_t, 0, "Parameter for the motion model", 0.0, 0.0, 30.0)
gen.add("a_12", double_t, 0, "Parameter for the motion model", 1.0, 0.0, 30.0)

gen.add("lin_scale", double_t, 0, "Linear scale of the motion update that only applies noise to the particles", 0.1, 0.0, 10.0)
gen.add("ang_scale", double_t, 0, "Angular scale of the motion update that only applies noise to the particles", 0.1, 0.0, 10.0)

evaluation_enum = gen.enum([ gen.const("Naiv",      int_t, 0, "Naiv range evaluation"),
                             gen.const("Likelihood",     int_t, 1, "Likelihood range evaluation")],
                             "An enum to set range evaluation model")

resampling_enum = gen.enum([ gen.const("Wheel",                 int_t, 0, "Wheel Resampler"),
                             gen.const("Residual",              int_t, 1, "Residual Resampler"),
                             gen.const("Systematic",            int_t, 2, "Systematic Resampler"),
                             gen.const("ResidualSystematic",   int_t, 3, "Residual Systematic Resampler"),
                             gen.const("Metropolis",            int_t, 4, "Metropolis Resampler"),
                             gen.const("RejectionSystematic",  int_t, 5, "Rejection Resampler")],
                             "An enum to set the resampling method")

gen.add("evaluation_model", int_t, 0, "Evaluation model for evaluation the difference of an observed and a simulated scan", 1, 0, 1, edit_method=evaluation_enum)

gen.add("resampling_method", int_t, 0, "Method for the resampling step in the particle filter", 1, 0, 5, edit_method=resampling_enum)

exit(gen.generate(PACKAGE, "tsdf_localization", "MCL"))